/*
 * @Author: yy.yy 729853861@qq.com
 * @Date: 2023-02-08 13:55:06
 * @LastEditors: yy.yy 729853861@qq.com
 * @LastEditTime: 2023-02-16 15:45:30
 * @FilePath: \ctr_simulition\mpc_ctrl.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "mpc_ctrl.h"
#include <iostream>

Parament_mpc::Parament_mpc(const Ref<const MatrixXf>& a,const Ref<const MatrixXf>& b,const Ref<const MatrixXf>& q,
    const Ref<const MatrixXf>& r,const Ref<const MatrixXf>& f,int N)
{
    m_A = a ; 
    m_B = b;
    m_Q = q;
    m_R = r;
    m_F = f;
    m_size_n = m_A.rows();
    m_size_p = m_B.cols();
    m_size_N = N ; 
}
void Parament_mpc::Print_size()
{    
    std::cout << "size_n: " << m_size_n <<std::endl ;
    std::cout << "size_N: " << m_size_N <<std::endl ;
    std::cout << "size_p: " << m_size_p <<std::endl ;
}
void Parament_mpc::Calculation_mpc(const Ref<const MatrixXf>& a , const Ref<const MatrixXf>& b)
{
    MatrixXf tmp_A = a ; 
    MatrixXf tmp_AB = MatrixXf::Zero(m_size_n,m_size_N*m_size_p) ; 
    MatrixXf tmp_B = b;
    m_Qbar = MatrixXf::Identity(m_size_n*(m_size_N+1),m_size_n*(m_size_N+1));
    m_Rbar = MatrixXf::Identity(m_size_p*m_size_N,m_size_p*m_size_N);
    m_M.resize((m_size_N+1)*m_size_n,m_size_n);
    m_C.resize((m_size_N+1)*m_size_n,m_size_N*m_size_p);
    for(int i = 0 ; i<m_size_N+1 ; i++)
    {
        if(i == 0)
        {
            m_Qbar.block(i*m_size_n,i*m_size_n,m_size_n,m_size_n) = m_Q;
            m_Rbar.block(i*m_size_n,i*m_size_n,m_size_n,m_size_n) = m_R;
            m_M.block(0,0,m_size_n,m_size_n) = MatrixXf::Identity(m_size_n,m_size_n) ;
            m_C.block(0,0,m_size_n,m_size_N*m_size_p) = MatrixXf::Zero(m_size_n,m_size_N*m_size_p);           
        } 
        else
        {
            //计算M矩阵
            m_M.block(i*m_size_n,0,m_size_n,m_size_n) = tmp_A;
            tmp_A = a*tmp_A;
            //计算C矩阵
            for(int j = 0 ; j < i ; j++)
            {
                if(j == 0 ) tmp_B = b;
                else 
                {
                    tmp_B = a * tmp_B ; 
                }
                tmp_AB.block(0,(i-j-1)*m_size_p,m_size_n,m_size_p) = tmp_B ; 
            }
            m_C.block(i*m_size_n,0,m_size_n,m_size_N*m_size_p) = tmp_AB;
            //计算QR
            if(i == m_size_N)
            {
                m_Qbar.block(i*m_size_n,i*m_size_n,m_size_n,m_size_n) = m_F;
                     
            }
            else
            {
                m_Qbar.block(i*m_size_n,i*m_size_n,m_size_n,m_size_n) = m_Q;
                m_Rbar.block(i*m_size_p,i*m_size_p,m_size_p,m_size_p) = m_R;
            }

            
        }   
    }
    m_E = m_C.transpose() * m_Qbar * m_M;
    m_H = m_C.transpose() * m_Qbar * m_C + m_Rbar;
};
MatrixXf Parament_mpc::Prediction(const Ref<const MatrixXf>& a , const Ref<const MatrixXf>& b,const Ref<const MatrixXf>& x0)
{
    Calculation_mpc(a,b); //计算E、H矩阵
    m_uk = Quadprog(m_H,m_E * x0).block(0,0,m_size_p,1);
    return m_uk;
};
MatrixXf Parament_mpc::Quadprog(const Ref<const MatrixXf>& H , const Ref<const MatrixXf>& f)
{
    MatrixXf tmp_ans = H.inverse() * (-f) ;
    return tmp_ans ; 
}
int main()
{

    MatrixXf a(2,2);
    MatrixXf b(2,2);
    MatrixXf q(2,2);
    MatrixXf r(2,2);
    MatrixXf f(2,2);
    MatrixXf x0(2,1);
    MatrixXf tmp ; 
    x0 << 20,-20;
    f << 100,0,0,1;
    r << 1,0,0,0.1;
    q << 100, 0 ,0,1;
    a <<1,0.1,-1,2;
    b << 0.2,1,0.5,2;
    int N = 5;
    Parament_mpc mpc(a,b,q,r,f,N);
    mpc.Print_size();
    for(int i = 0; i < 100 ; i++)
    {
        tmp = mpc.Prediction(a,b,x0);
        std::cout << "输入" << tmp <<std::endl;
        x0 = a * x0 + b * mpc.m_uk ; 

    }
   
}